#include <controller/controller_node.hpp>

namespace controller
{

ControllerNode::ControllerNode(int argc, char **argv) : rate(20)
{
    ros::NodeHandle nh;
    state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10,std::bind(&ControllerNode::state_callback, this, std::placeholders::_1));
    local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 1;

    while(ros::ok() && !current_state.connected){
        ros::spinOnce();       //处理一次回调
        ROS_INFO("waiting for FCU connection...");
        rate.sleep();          //控制频率在20Hz
    }
    // std::thread callback_thread(std::bind(&ControllerNode::callback_handler, this));  //启动回调线程
    spinner = std::make_unique<ros::AsyncSpinner>(1);
    spinner->start();

}

ControllerNode::~ControllerNode()
{
    // callback_thread.detach();   //销毁线程
}

void ControllerNode::state_callback(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
    ROS_INFO("state_callback");
}

void ControllerNode::callback_handler(void)
{
    while(ros::ok())
    {
        ros::spinOnce();       //处理一次回调
        ROS_INFO("callback_thread");
        std::this_thread::sleep_for(std::chrono::milliseconds(20));
    }
}

void ControllerNode::arm_control(bool arm)
{   
    arm_cmd.request.value = arm;

    while( current_state.armed != arm)
    {
        ROS_WARN("mismatch arm status %s",arm ?  "true" : "false");

        if( arming_client.call(arm_cmd) && arm_cmd.response.success)
        {
            ROS_INFO("Vehicle armed %s",arm ?  "true" : "false");
        }
        rate.sleep();
    }
}


void ControllerNode::mode_control(std::string mode)
{
    set_mode.request.custom_mode = mode;
    
    while( current_state.mode != mode )
    {
        ROS_WARN("expect mode: %s  current mode: %s",mode,current_state.mode);

        if( set_mode_client.call(set_mode) && set_mode.response.mode_sent)
        {
            ROS_INFO("%s enabled",mode);
        }
        rate.sleep();
    }
}

void ControllerNode::position_control(float x, float y, float z)
{
    pose.header.stamp = ros::Time::now();
    pose.pose.position.x = x;
    pose.pose.position.y = y;
    pose.pose.position.z = z;
    local_pos_pub.publish(pose);
}


}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "control_node");
    controller::ControllerNode controller_node(argc, argv);
    controller_node.arm_control(true);




    while(ros::ok())
    {           
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
}